#! /opt/local/bin/python2.6
import numpy as np
import time
import sys
from rwEXR import *


# 3744 23.9
# 5616 35.8


# distortion correction parameters in rad
# 0.207029529537 0.0422547753997  
entrancePixel = (2323,5615-2427) #5
entrancePixel = (2622,5615-1935) #6
entrancePixel = (3123,5615-1859) #7
entrancePixel = (3479,5615-2224) #1
entrancePixel = (3341,5615-2345) #2
entrancePixel = (3127,5615-2538) #3
entrancePixel = (3616,5615-3037) #4





physicCenter = (1867.5,5615-2818.5)
#center = (1884.18,2834.89)
#center = np.array([2836.5,3744-1860.79])

#center = (1853,5615-2874.5)
distortionCenter = (1860.79,5615-2836.5)
distortionCenter = (1864,5615-2834)

#center = np.array([2836.5,3744-1860.79])


def Rd_distortion(pixel):
    c = distortionCenter
    return np.sqrt( (pixel[0] - c[0])*(pixel[0] - c[0]) + (pixel[1] - c[1])*(pixel[1] - c[1]) )

def Rd_physic(pixel):
    c = physicCenter
    return np.sqrt( (pixel[0] - c[0])*(pixel[0] - c[0]) + (pixel[1] - c[1])*(pixel[1] - c[1]) )


pix = np.sqrt(23.9*23.9 + 35.8*35.8)/np.sqrt(3744*3744+5616*5616)
print pix

#coeffs = [ -1.54035227e-12 ,  6.94350405e-09  ,-1.22082544e-05,   9.47349416e-03,5.90440253e+00]
#coeffs = [ -1.94777642e-12 ,  9.17138801e-09 , -1.59443120e-05 ,  1.21270728e-02,5.15110092e+00]
coeffs = [ -1.48631764e-12  , 6.66110873e-09 , -1.16207010e-05 ,9.22816587e-03,5.78973689e+00]
coeffs = [ -2.69459533e-12,   1.19761622e-08  ,-1.99256846e-05 ,  1.46539282e-02,4.54699550e+00]

poly_Rd2f = np.poly1d(coeffs)

f = poly_Rd2f(Rd_distortion(entrancePixel)) - 0.5
print Rd_distortion(entrancePixel)
print f

d = 79+89

deg=90.3

T1 = np.matrix([d*np.sin(deg*np.pi/180.0),+d*np.cos(deg*np.pi/180.0)-d,0])

#R = np.matrix([[0,-1,0],[1,0,0],[0,0,1]])

#e1 = np.matrix([-f,d+f,0]) 

#e2 = np.matrix([d+f,-f,0])

#theta = 2*np.arcsin( Rd_distortion(entrancePixel)  / (2.0 * f/pix) )

#phi   = np.arctan2( entrancePixel[1] - distortionCenter[1], entrancePixel[0] - distortionCenter[0] )

#op1 = np.matrix([ np.sin(theta)*np.cos(phi), np.cos(theta), np.sin(theta)*np.sin(phi)])

XX = (entrancePixel[0] - distortionCenter[0]) / Rd_distortion(entrancePixel)*f * np.tan(2.0*np.arcsin(Rd_distortion(entrancePixel)/(2.0*f/pix))) + 3.5 * pix
ZZ = (entrancePixel[1] - distortionCenter[1]) / Rd_distortion(entrancePixel)*f * np.tan(2.0*np.arcsin(Rd_distortion(entrancePixel)/(2.0*f/pix))) + 16.0 * pix


op1 = np.matrix([XX,8,ZZ])
#E = np.matrix( [[0,0,-d],[0,0,-d],[d,-d,0]])
N = np.cross(op1,T1) 

print N

#N = (op1 * E)

N = N.flat

n1 = N[0]
n2 = N[1]
n3 = N[2]

#n4 = -n1*(d+8) + n2*8

y = np.hstack((np.arange(-1500,-500,5),np.arange(-500,-200,0.2),np.arange(-200,200,0.001),np.arange(200,500,0.2),np.arange(500,1500,5))) - 3.5 * pix

#z = (-n2*f-n2*y)/(n3) + 16 * pix

z = (n2*d - n1*(d+8 - np.cos(deg*np.pi/180.0)*y)/np.sin(deg*np.pi/180.0)-n2*y)/n3 - 16.0 * pix


ru = np.sqrt(z*z+y*y)
outPixel = (567,5615-2354)  #5
outPixel = (1058,5615-1910) #6
outPixel = (1609,5615-1977) #7
outPixel = (1843,5615-2341) #1
outPixel = (1621,5615-2415) #2
outPixel = (1294,5615-2555) #3
outPixel = (2005,5615-2998) #4



f = poly_Rd2f(Rd_distortion(outPixel)) -0.5
print f


rd = 2*f*np.sin(np.arctan2(ru,f)/2)

U = -rd * np.cos(np.arctan2(z,y)) / pix 
V =  rd * np.sin(np.arctan2(z,y)) / pix 

exrR,exrG,exrB,L,size = readExr("/Network/scratch/Tests/XL/DepthEstimationProject/sourceimages/HDRI/rotationFisheye/exr/vue1.exr")

for (u,v) in zip(U,V):
    try:
        L[u+physicCenter[0],v+physicCenter[1]] = 255
    except:
        pass

createNewOutputImage("epipolar.exr",L.T,L.T,L.T,size)

#print e1 * E 



















